AP_ExternalControl: initial implementation of external control library #24549

tridge · 2023-08-09T00:49:17Z

This is a very basic implementation of an external control library, just to unblock @pedro-fuoco

Relates to #23363

Testing Instructions

Run sim_vehicle with --enable-dds --console --map
Set DDS_ENABLED param to 1
Use mavproxy to arm the vehicle, set the mode to guided, and takeoff to ~50m
Run the microROS agent like normal
Verify with ros2 CLI that you see the ap/cmd_vel topic
Publish commands to ap/cmd_vel, or use rqt. Current behavior is this
```
# Go up at 1 m/s
ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{twist: {linear: {z: 1.0}}}'
# Go down at 2 m/s
ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{twist: {linear: {z: -1.0}}}'
# Go east at 1 m/s
ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{twist: {linear: {x: 1.0}}}'
# Rotate the quadcopter 180 degrees and repeat the above tests. Regardless of the orientation, x always makes the copter go north because the cmd_vel topic is odom frame ENU.
```
We do need to decide if the cmd_vel topic should be in body frame or map frame. According to REP-147, the cmd_vel topic i in body frame. Perhaps we should use the frame ID to either body or map for these different ones? Or a different topic? We have to decide based on clarity (topics are easier to notice than frame ID's), but additional subscribers impact flash, code size, and RAM/CPU usage. https://ros.org/reps/rep-0147.html#rate-interface

Ryanf55 · 2023-08-09T03:22:18Z

libraries/AP_ExternalControl/AP_ExternalControl.h

+    /*
+      set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
+      velocity is in earth frame, NED, m/s
+     */


Should also say the frame convention for yaw rate? Even @lthall has realized the mavlink API's are not quite cut and dry. Although I don't recommend reading too much of this thread, this comment is useful.
mavlink/mavlink#2013 (comment)

Yeh, I am going to put a PR in to fix this mistake. Once I saw it I wonder why I missed it the first time.

Nice. Please tag me, so we can follow the same convention here.

libraries/AP_DDS/AP_DDS_Client.cpp

pedro-fuoco · 2023-08-09T21:55:54Z

libraries/AP_DDS/AP_DDS_Client.cpp

+        Vector3f linear_velocity {
+            float(rx_velocity_control_topic.twist.linear.y),
+            float(rx_velocity_control_topic.twist.linear.x),
+            float(-rx_velocity_control_topic.twist.linear.z) };
+        const float yaw_rate = -rx_velocity_control_topic.twist.angular.z;
+        external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);


Here are the test results for the frame orientations:
Positive x (correctly went East):

Positive y (correnctly went North):

Negative z (Wrongfully went Up):

Positive Yaw (correctly went CounterClockwise looking from above):

Thus, simply changing the linear z axis should solve it.

Suggested change

Vector3f linear_velocity {

float(rx_velocity_control_topic.twist.linear.y),

float(rx_velocity_control_topic.twist.linear.x),

float(-rx_velocity_control_topic.twist.linear.z) };

const float yaw_rate = -rx_velocity_control_topic.twist.angular.z;

external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);

Vector3f linear_velocity {

float(rx_velocity_control_topic.twist.linear.y),

float(rx_velocity_control_topic.twist.linear.x),

float(rx_velocity_control_topic.twist.linear.z) };

const float yaw_rate = -rx_velocity_control_topic.twist.angular.z;

external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);

Also tested to see if the frame orientations were consistent with #23541. Everything except the z linear axis is perfect

Here are the test results for the frame orientations: Positive x (correctly went East): Positive y (correnctly went North): Negative z (Wrongfully went Up): Positive Yaw (correctly went CounterClockwise looking from above):

Thus, simply changing the linear z axis should solve it.

This negative for Z should go in the copter API. The external AHRS said positive Z is down, and copter implements it differently.

This negative for Z should go in the copter API. The external AHRS said positive Z is down, and copter implements it differently.

Yes, we should definatly be using NED where positive Z is down. It can get a bit mixed up because positive altitude is up and I have not converted the position controller to NED yet but I have done most of the work in the background to make that happen. I intend it to be done by the time we fix the units.

The suggestion from Pedro would solve it for Copter but break other vehicles. I have added a negative in the Copter implementation in this commit, as well as a comment why.
b4251a1

If/When copter transitions to NED for velocity control, the interface in AP_ExternalControl will not change, because it's already NED.

@pedro-fuoco In my testing, I see positive X going west.

Ryanf55 · 2023-08-10T03:19:54Z

After taking a deeper review:

How should the API handle if the user commands a higher velocity than the aircraft is configured to do?
Is this API method valid for rover and plane? Probably not...
Should there be a define around the code to enable it?

Ryanf55 · 2023-08-11T04:25:30Z

After some discussion with @lthall and Steve (lead of ROS2 navigation framework), and reading REP147, we're put in an interesting position.

REP-147 recommends body frame control, and says that rovers used earth frame, but it was less flexible. The second part seems out of date give 2. The command is a body relative set of velocities in linear and angular space. This diverges from the common geometry_msgs/Twist [[26]](https://ros.org/reps/rep-0147.html#twist) used by ground robots. A pure Twist based inteface could be provided for backwards compatability as well. It was chosen to diverge here since this is a much more powerful inteface for the vehicles and does not require all commands to be reverse calculated for the instantaneous attitude of the vehicle.
NAV2 uses body frame control
ArduCopter uses earth frame control because the math has to compensate for gravity. From Leonard: Practically, control is always done in the earth frame because gravity forces the vertical to be handled differently to the horizontal.
In ROS, the translation from map to base_link is not guaranteed to be consistent, but ArduPilot has a way to deal with this.

Given these conflicts, we decided the following is a best approach for the first PR:

This PR will be left as-is because earth-frame control works great on copter and is easy to understand.
You are required to set the frame_id to map to use ROS 2 earth frame control in ArduPilot.
We can choose to implement other vehicles or body-frame control based on user's interest.
Rhys and I want some time to set up some test infrastructure before we get too far down road adding all the features.

Ryanf55 · 2023-08-11T13:39:30Z

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

+}
+
+
+#endif // AP_DDS_ENABLED


Missing newline.

ArduCopter/AP_ExternalControl_Copter.cpp

pedro-fuoco

Just re-tested everything with the ros2 cli. ENU is being followed perfectly and it is nice to filter the messages by frame_id (in this case, ignoring everything that isn't "map" frame). It will make the body-frame version of this easier to implement.

The command I used to test it was:

ros2 topic pub /ap/cmd_vel geometry_msgs/msg/TwistStamped '{header: {frame_id: "map"}, twist: {linear: {x: 1}}}'

LGTM!

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

tridge added WIP ROS labels Aug 9, 2023

tridge requested review from rmackay9, Ryanf55 and pedro-fuoco August 9, 2023 00:51

Ryanf55 added the AllVehicles label Aug 9, 2023

tridge force-pushed the pr-external-control branch from 4d61d20 to c6a18da Compare August 9, 2023 00:59

Ryanf55 reviewed Aug 9, 2023

View reviewed changes

pedro-fuoco reviewed Aug 9, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

pedro-fuoco suggested changes Aug 9, 2023

View reviewed changes

Ryanf55 force-pushed the pr-external-control branch from c6a18da to 657d9da Compare August 9, 2023 22:12

Ryanf55 force-pushed the pr-external-control branch from bc92988 to 8185ba6 Compare August 10, 2023 03:58

Ryanf55 requested a review from srmainwaring August 10, 2023 20:36

Ryanf55 mentioned this pull request Aug 11, 2023

Fix copy-paste error in velocity control ros-infrastructure/rep#384

Merged

Ryanf55 reviewed Aug 11, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

}

#endif // AP_DDS_ENABLED

Copy link

Collaborator

Ryanf55 Aug 11, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Missing newline.

This was referenced Aug 11, 2023

Proposal: Support TwistStamped Velocity Control for REP-147 (Aerial) applications ros-navigation/navigation2#3755

Closed

Switch from Twist to TwistStamped for cmd_vel ros-navigation/navigation2#1594

Open

tridge commented Aug 15, 2023

View reviewed changes

ArduCopter/AP_ExternalControl_Copter.cpp Outdated Show resolved Hide resolved

Ryanf55 force-pushed the pr-external-control branch from 7d4f687 to f5f491b Compare August 16, 2023 05:29

pedro-fuoco approved these changes Aug 18, 2023

View reviewed changes

Ryanf55 force-pushed the pr-external-control branch from ac46597 to d5b1ef9 Compare August 18, 2023 01:47

tridge and others added 3 commits August 17, 2023 19:48

AP_ExternalControl: external control library for MAVLink,lua and DDS

e72467c

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

Copter: added AP_ExternalControl support

d409f5d

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

waf: added AP_ExternalControl to set of libraries

386b057

Ryanf55 force-pushed the pr-external-control branch from d5b1ef9 to 38a796d Compare August 18, 2023 01:51

Ryanf55 added the DevCallTopic label Aug 21, 2023

CraigElder removed the DevCallTopic label Aug 21, 2023

rmackay9 approved these changes Aug 21, 2023

View reviewed changes

tridge added MergeOnCIPass and removed WIP labels Aug 21, 2023

tridge and others added 4 commits August 21, 2023 17:11

AP_DDS: use AP_ExternalControl for velocity control

58b3319

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

Plane: dummy AP_ExternalControl implementation

5f46526

Rover: dummy implementation of AP_ExternalControl

31a92c6

Tools: Enforce astyle on AP_ExternalControl

b9afe37

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

Ryanf55 force-pushed the pr-external-control branch from 38a796d to b9afe37 Compare August 21, 2023 23:11

tridge merged commit d76bf32 into ArduPilot:master Aug 22, 2023
81 checks passed

srmainwaring removed the MergeOnCIPass label Feb 27, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_ExternalControl: initial implementation of external control library #24549

AP_ExternalControl: initial implementation of external control library #24549

tridge commented Aug 9, 2023 •

edited by Ryanf55

Ryanf55 Aug 9, 2023

lthall Aug 10, 2023

Ryanf55 Aug 10, 2023

pedro-fuoco Aug 9, 2023

pedro-fuoco Aug 9, 2023

Ryanf55 Aug 9, 2023

lthall Aug 10, 2023

Ryanf55 Aug 10, 2023 •

edited

Ryanf55 Aug 10, 2023

Ryanf55 commented Aug 10, 2023

Ryanf55 commented Aug 11, 2023 •

edited

Ryanf55 Aug 11, 2023

pedro-fuoco left a comment

AP_ExternalControl: initial implementation of external control library #24549

AP_ExternalControl: initial implementation of external control library #24549

Conversation

tridge commented Aug 9, 2023 • edited by Ryanf55

Testing Instructions

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Ryanf55 Aug 10, 2023 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Ryanf55 commented Aug 10, 2023

Ryanf55 commented Aug 11, 2023 • edited

Choose a reason for hiding this comment

pedro-fuoco left a comment

Choose a reason for hiding this comment

tridge commented Aug 9, 2023 •

edited by Ryanf55

Ryanf55 Aug 10, 2023 •

edited

Ryanf55 commented Aug 11, 2023 •

edited